#include "rmcsmotor.h"
#include <Arduino.h>
#include <EEPROM.h>

class HardwareSerial;

RmcsMotor::RmcsMotor()
{
  meanposition=0;
}

void RmcsMotor::attach(HardwareSerial *serial)
{
	serial->begin(9600);
        serialport = serial;
//        Serial.println("Serial Port 1 Started");
//	sprintf(buffer,"000000000000");
//	currentposition=getCurrentPosition();
//	int i=0;
//	int address=BASE_MOTOR_ADDRESS;
//	while(buffer[i]!='#')
//	{
//		buffer[i]=EEPROM.read(address);
//		if(buffer[i]==255) //EEPROM has never been written on
//			{
//				meanposition=currentposition;
//				return;
//			}
//		i++;
//		address++;
//	}
////	buffer[i]='\0';
//	sscanf(buffer,"%g",&meanposition);
}
	
void RmcsMotor::rotateRelative(int degree)
{
	int delta=degree*5; //resolution of encoder is 0.2 deg
//	currentposition=getCurrentPosition();
//	int newposition=currentposition+delta;
//	sprintf(buffer,"P%g",newposition);
	sprintf(buffer,"G%d",delta);  //G for Relative Position
	serialport->println(buffer);
}

void RmcsMotor::rotateAbsolute(int degree)
{
	double newposition=degree*5;
//	serialport->println("Writing on Serial Port");
        sprintf(buffer,"P%d",newposition+meanposition);
        //Serial.println(buffer);
        
        //serialport->println(buffer);
}
	
double RmcsMotor::getCurrentPosition()
{
	serialport->println("P"); //query for current position
	int nofcharread=serialport->readBytesUntil('\n',buffer,12); 
	buffer[nofcharread]='\0';
	sscanf(buffer,"%lf",&currentposition);
        return currentposition;
}
int RmcsMotor::getCurrentAngle()
{
	currentposition=getCurrentPosition();
	return (currentposition-meanposition)/5;
}
void RmcsMotor::setCurrentPositionAsMean()
{
	currentposition=getCurrentPosition();
//	sprintf(buffer,"%g",currentposition);
//	int i=0;
//	int address=BASE_MOTOR_ADDRESS;
//	while(buffer[i]!='\0')
//	{
//		EEPROM.write(address,buffer[i]);
//		i++;
//		address++;
//	}
//	EEPROM.write(address,'#');
	meanposition=currentposition;
}
